package sep.test;

import static com.googlecode.javacv.cpp.opencv_core.cvTranspose;
import static com.googlecode.javacv.cpp.opencv_core.cvMatMul;
import static com.googlecode.javacv.cpp.opencv_core.cvSetIdentity;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.IOException;
import java.io.Serializable;
import java.util.ArrayList;
import java.util.List;

import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D32f;
import com.googlecode.javacv.cpp.opencv_core.CvPoint3D32f;

public abstract class IdaCameraModel implements Serializable {

	private static final long serialVersionUID = -4966998660901104269L;

	enum CameraModelType {
		PinHole
	}

	/**
	 * Intrinsische Parameter nochmal einzeln, da CvMat nicht serialisiert wird
	 */
	private double a;
	private double b;
	private double c;
	private double u0;
	private double v0;

	/**
	 * Intrinsische Parameter zur Umrechnung in Strassenmodell Koordinaten
	 */
	protected transient CvMat matCameraInv;

	/**
	 * Inverse intrinsische Parameter nochmal einzeln, da CvMat nicht
	 * serialisiert wird
	 */
	private double invA;
	private double invB;
	private double invC;
	private double invU0;
	private double invV0;

	/**
	 * Extrinsische Parameter
	 */
	private double wx;
	private double wy;
	private double wz;
	protected double translationX;
	protected double translationY;
	protected double translationZ;
	protected double scale;

	protected transient CvMat vecTranslationInv;
	protected transient CvMat matRotationInv;

	protected transient CvMat matP;
	protected transient CvMat matK;
	protected transient CvMat matR;

	public abstract CvPoint2D32f toCameraCoordinates(final double x,
			final double y, final double z);

	public abstract CvPoint3D32f toStreetCoordinates(final double x,
			final double y);

	public abstract CameraModelType getModelType();

	public void setRotationAngles(final double x, final double y, final double z) {
		wx = x;
		wy = y;
		wz = z;
		calcRotationMatrix();
	}

	public void setTranslation(final double x, final double y, final double z) {

		translationX = x;
		translationY = y;
		translationZ = z;

		synchronized (vecTranslationInv) {
			vecTranslationInv.put(0, 0, -x);
			vecTranslationInv.put(1, 0, -y);
			vecTranslationInv.put(2, 0, -z);
		}

		synchronized (matR) {
			matR.put(0, 3, x);
			matR.put(1, 3, y);
			matR.put(2, 3, z);
		}
		
		// Projektionsmatrix berechnen
		cvMatMul(matK, matR, matP);
	}

	public double[] getRotationAngles() {
		return new double[] { wx, wy, wz };
	}

	public double[] getTranslation() {
		return new double[] { translationX, translationY, translationZ };
	}

	public void setCameraParameters(final double a, final double b,
			final double c, final double u0, final double v0) {

		this.a = a;
		this.b = b;
		this.c = c;
		this.u0 = u0;
		this.v0 = v0;

		synchronized (matK) {
			matK.put(0, 0, a);
			matK.put(0, 1, c);
			matK.put(1, 1, b);
			matK.put(0, 2, u0);
			matK.put(1, 2, v0);
		}
		
		// Projektionsmatrix berechnen
		cvMatMul(matK, matR, matP);
	}

	public void setCameraParametersInv(final double a, final double b,
			final double c, final double u0, final double v0) {

		this.invA = a;
		this.invB = b;
		this.invC = c;
		this.invU0 = u0;
		this.invV0 = v0;

		synchronized (matCameraInv) {
			matCameraInv.put(0, 0, a);
			matCameraInv.put(0, 1, c);
			matCameraInv.put(1, 1, b);
			matCameraInv.put(0, 2, u0);
			matCameraInv.put(1, 2, v0);
		}
	}

	public double[] getCameraParameters() {
		synchronized (matK) {
			// Reihenfolge: a, b, c, u0, v0
			return new double[] { matK.get(0, 0), matK.get(1, 1),
					matK.get(0, 1), matK.get(0, 2), matK.get(1, 2) };
		}
	}

	private List<Double> extractValues(final String line) {
		final String[] splits = line.split(" ");

		final List<Double> vals = new ArrayList<Double>();

		for (String s : splits) {
			if (!s.isEmpty()) {
				vals.add(Double.parseDouble(s));
			}
		}

		return vals;
	}

	public boolean loadParameters(final File file) {
		if (file == null) {
			throw new IllegalArgumentException("File must not be null");
		}

		BufferedReader reader = null;

		try {
			reader = new BufferedReader(new FileReader(file));

			// 4 Zeilen �berspringen
			for (int i = 0; i < 4; i++) {
				reader.readLine();
			}

			// Parameter f�r Projektion
			// a, b, c, u0, v0 laden
			String line = reader.readLine();
			List<Double> vals = extractValues(line);
			setCameraParameters(vals.get(0), vals.get(1), vals.get(4),
					vals.get(2), vals.get(3));

			// 1 Zeile �berspringen
			reader.readLine();

			// Radiale Parameter laden
			line = reader.readLine();
			vals = extractValues(line);

			// 1 Zeile �berspringen
			reader.readLine();

			// Tangentiale Parameter werden nicht verwendet
			line = reader.readLine();

			// 1 Zeile �berspringen
			reader.readLine();

			// Prismische Parameter werden nicht verwendet
			line = reader.readLine();

			// 1 Zeile �berspringen
			reader.readLine();

			// Parameter f�r Reprojektion
			// a, b, c, u0, v0 laden
			line = reader.readLine();
			vals = extractValues(line);
			setCameraParametersInv(vals.get(0), vals.get(1), vals.get(4),
					vals.get(2), vals.get(3));

			// 1 Zeile �berspringen
			reader.readLine();

			// Radiale Parameter laden
			line = reader.readLine();
			vals = extractValues(line);

			return true;

		} catch (FileNotFoundException e) {
			System.out.println(e.getMessage());

		} catch (IOException e) {
			System.out.println(e.getMessage());

		} finally {
			if (reader != null) {
				try {
					reader.close();
				} catch (IOException e) {
					System.out.println(e.getMessage());
				}
			}
		}

		return false;
	}

	private void calcRotationMatrix() {
		// Rotation um x-Achse
		final CvMat Rx = CvMat.create(3, 3);
		double radians = Math.toRadians(wx);
		double sin = Math.sin(radians);
		double cos = Math.cos(radians);
		cvSetIdentity(Rx, 0.0);
		Rx.put(0, 0, 1.0);
		Rx.put(1, 1, cos);
		Rx.put(1, 2, -sin);
		Rx.put(2, 1, sin);
		Rx.put(2, 2, -cos);

		// Rotation um y-Achse
		final CvMat Ry = CvMat.create(3, 3);
		radians = Math.toRadians(wy);
		sin = Math.sin(radians);
		cos = Math.cos(radians);
		cvSetIdentity(Ry, 0.0);
		Ry.put(0, 0, cos);
		Ry.put(0, 2, sin);
		Ry.put(1, 1, 1.0);
		Ry.put(2, 0, -sin);
		Ry.put(2, 2, cos);

		// Rotation um y-Achse
		final CvMat Rz = CvMat.create(3, 3);
		radians = Math.toRadians(wz);
		sin = Math.sin(radians);
		cos = Math.cos(radians);
		cvSetIdentity(Rz, 0.0);
		Rz.put(0, 0, cos);
		Rz.put(0, 1, -sin);
		Rz.put(1, 0, sin);
		Rz.put(1, 1, cos);
		Rz.put(2, 2, 1.0);

		synchronized (matRotationInv) {
			cvMatMul(Rz, Ry, matRotationInv);
			cvMatMul(matRotationInv, Rx, matRotationInv);

			synchronized (matR) {
				matR.put(matRotationInv);
			}

			cvTranspose(matRotationInv, matRotationInv);
		}
		
		// Projektionsmatrix berechnen
		cvMatMul(matK, matR, matP);
	}

	public void create() {
		matCameraInv = CvMat.create(3, 3);
		vecTranslationInv = CvMat.create(3, 1);
		matRotationInv = CvMat.create(3, 3);

		matK = CvMat.create(4, 4);
		matP = CvMat.create(4, 4);
		matR = CvMat.create(4, 4);

		setRotationAngles(wx, wy, wz);
		setTranslation(translationX, translationY, translationZ);
		setCameraParameters(a, b, c, u0, v0);
		setCameraParametersInv(invA, invB, invC, invU0, invV0);
	}
}
